/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
 * This object implements a path planning algorithm based on CMA-ES. The user
 * must provide the necessary input data (see constructor).
 *                      
 * FILENAME:
 * planner.h
 *
 * PURPOSE:
 * To implement a path planner object based around the CMA-ES algorithm.
 *
 * CREATED:
 * 08/2011 by Maciej Stachura
 *
*/ 

#ifndef _PLANNER_H
#define _PLANNER_H

Matrix ga_randomSoln(Matrix LB, Matrix UB);
Matrix ga_produceFromPop(Matrix oldpop, Matrix newpop,Matrix slots, Matrix LB, Matrix UB);
size_t findSlot(int v, Matrix slots);
double uniform();
int randi(int min, int max);

#define PLANNER_TYPE 0 // 0 = CMAES, 1 = GA

#include <math.h>
#include <iostream>
#include "timeLib.h"
#include "cmaes_interface.h"

using namespace std;

template <class plan_t>
class planner
{
  public:
    plan_t param; // Parameter struct for data needed by the cost function.

    //Constructor
    planner ( double (*J_)(double*, plan_t), plan_t param_ ) {J = J_; param = param_;}

    double (*J)(double*, plan_t); // Pointer to cost function.
    void init();
    double* update(double *);
    Matrix ga(Matrix LB, Matrix UB, double tstop, int popSize, Matrix pop);

  private:
    cmaes_t evo;
    char const* fname;
    double *xbestever; // store best solution
    double stopTolFun, stopTolX;
};

template <class plan_t>
void planner<plan_t>::init()
{
	srand(time(NULL));
  xbestever=NULL;
  stopTolFun = 0.1;
  stopTolX = 0.1;
}

template <class plan_t>
double* planner<plan_t>::update(double * u_init)
{
#if(PLANNER_TYPE==0)
  double *const*pop; // sampled population
  double *fitvals;   // objective function values of sampled population
  int countevals = 0, i = 0;  // offspring population size, 0 invokes default
  char const * stop; // stop message
  
  fitvals = cmaes_init(&evo, param.N, u_init, param.inrgstddev, 0, 0, stopTolFun, stopTolX);
  evo.countevals = countevals; // a hack, effects the output and termination

  while(!(stop=cmaes_TestForTermination(&evo)))
  { 
    // Generate population of new candidate solutions
    pop = cmaes_SamplePopulation(&evo); // do not change content of pop
    // Compute fitness value for each candidate solution
    for (i = 0; i < cmaes_Get(&evo, "popsize"); ++i) {
      // while (!is_feasible(pop[i])) 
      //   cmaes_ReSampleSingle(&evo, i);

      fitvals[i] = J(pop[i],param); 
    }
    cmaes_UpdateDistribution(&evo, fitvals); // update search distribution
  }
  return cmaes_GetInto(&evo, "xbestever", xbestever);
//#else

#endif
}

template <class plan_t>
Matrix planner<plan_t>::ga(Matrix LB, Matrix UB, double tstop, int popSize, Matrix pop)
{
  // Assuming the ga is looking for a minimum of the costFcn.
  // setup
  // we need a selection scheme, use a slotting scheme for preferencing
	TimeStamp tstart, tcurrent;
  Matrix selScheme;
  selScheme.Null(popSize,1);
  selScheme(0) = popSize;
  for(size_t i=1; i<selScheme.length(); i++)
    selScheme(i) = selScheme(i-1)+popSize+1-i;

  // initialize the population
  while(pop.NumRow() < popSize)
  {
    pop.SetSize(pop.NumRow()+1,pop.NumCol());
    pop.setRow(pop.NumRow(), ga_randomSoln(LB, UB)); // randomly insert a soln
  }

  // evaluate the solns
  // get the soln values
  Matrix values; values.Null(pop.NumRow(),1);
  double bestValue = 999999;
  Matrix bestSoln;
  double *u_tmp; u_tmp = new double[pop.NumCol()];
  for(size_t i=0; i<pop.NumRow(); i++)
  {
    pop.getRow(i,u_tmp);
    values(i) = J(u_tmp,param);
    if(values(i) < bestValue) // found new bestSoln
    {
      bestValue = values(i);
      bestSoln = pop.getRow(i);
    }
  }

  // loop until stopping criteria reached
  int genNum = 1;
  tstart.stamp();

  while(1)
  {
    tcurrent.stamp();
    if((tcurrent - tstart)/SEC2MICRO > tstop)
      return bestSoln;
    // sort the population by value
    //[values, vi] = sort(values);
    pop = pop(vi,:);

    // produce new generation
    Matrix prevGen = pop;
    pop.Null(0,pop.NumCol());
    while(pop.NumRow() < popSize)
      pop = ga_produceFromPop(prevGen, pop, selScheme, LB, UB);
    genNum++;

    // evaluate the population
    values.Null(pop.NumRow(),1);
    bestValue = 999999;
    for(size_t i=0; i < pop.NumRow(); i++)
    {
      pop.getRow(i,u_tmp);
      values(i) = J(u_tmp,param);
      if(values(i) < bestValue)
      {
        // found new bestSoln
        bestValue = values(i);
        bestSoln = pop.getRow(i);
      }
    }
  }
}

// randomSoln function
Matrix ga_randomSoln(Matrix LB, Matrix UB)
{
  //FIXME hardcoded for [-w_max 0 w_max] sort of
  Matrix soln(LB);
  for(size_t i = 0; i < soln.length(); i++)
    soln(i) = UB(i)*randi(-1,1);
  return soln;
}

// produceFromPop function
Matrix ga_produceFromPop(Matrix oldpop, Matrix newpop,
    Matrix slots, Matrix LB, Matrix UB)
{
  size_t r = 0;
  // select random mutation or from population for first mate
  Matrix soln1;
  if(uniform() < 0.8)
  {
    r = randi(0,(int)slots(slots.length())-1);
    soln1 = oldpop.getRow(findSlot(r,slots)); // select from population
  }else
    soln1 = ga_randomSoln(LB, UB); // create random first mate

  // select second mate from population
  r = randi(0,(int)slots(slots.length())-1);
  Matrix soln2 = oldpop.getRow(findSlot(r,slots));

  // use a single point crossover
  r = randi(0,soln1.length()-1);
  newpop.SetSize(newpop.NumRow()+2,newpop.NumCol());
  for(size_t i=0; i<newpop.NumCol(); i++)
  {
    if(i<r)
    {
      newpop(newpop.NumRow()-1, i) = soln1(i);
      newpop(newpop.NumRow(), i) = soln2(i);
    }else
    {
      newpop(newpop.NumRow()-1, i) = soln2(i);
      newpop(newpop.NumRow(), i) = soln1(i);
    }
  }
  return newpop;
}

// findSlot function
size_t findSlot(int v, Matrix slots)
{
  for (size_t vi=0; vi < slots.length(); vi++)
    if(v<slots(vi))
      return vi;
}

double uniform(){
	return ((double)rand() / (double)(RAND_MAX));
}

int randi(int min, int max){
	return floor((max-min+1)*uniform()) + min;
}

#endif
